package kit.edu.lego.gruppe3.levels.rotateBridge;

import kit.edu.lego.gruppe3.ComponentsRepository;
import kit.edu.lego.gruppe3.LegoMain;
import kit.edu.lego.gruppe3.bluetooth.control.GateCommon;
import kit.edu.lego.gruppe3.bluetooth.control.GateControl;
import kit.edu.lego.gruppe3.levels.LevelSelector;
import kit.edu.lego.gruppe3.levels.LevelSolver;
import kit.edu.lego.gruppe3.levels.bridge.BridgeSolver;
import kit.edu.lego.gruppe3.levels.door.DoorSolver;
import kit.edu.lego.gruppe3.levels.fallingBridge.FallingBridgeSolver;
import kit.edu.lego.gruppe3.levels.labyrinth.LabyrinthSolver;
import kit.edu.lego.gruppe3.levels.rollbridge.RollBridgeSolver;
import kit.edu.lego.gruppe3.levels.sumpf.SumpfSolver;
import kit.edu.lego.gruppe3.levels.treeArea.TreeAreaSolver;
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.subsumption.Arbitrator;
import lejos.robotics.subsumption.Behavior;
import lejos.util.Delay;

public class DiscSolver implements LevelSolver{
	
	private DifferentialPilot robot = ComponentsRepository.getInstance().getRobotPilot();
	private LightSensor lightS = ComponentsRepository.getInstance().getLightSensor();
	
	@Override
	public void start() {
		
		robot.rotate(170);
		robot.travel(-8.0f);

		while(lightS.readValue() < ComponentsRepository.LIGHT_TRASHOLD)
		{
			
		}
		
		Motor.B.rotate(180);
		robot.setAcceleration(1000);
		robot.setTravelSpeed(1000);
		
		robot.steer(100);
		try {
			Thread.sleep(2000);
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}



		while(true)
		{
			if(lightS.readValue() >= ComponentsRepository.LIGHT_TRASHOLD)
			{
				Motor.A.setSpeed(900);
				Motor.C.setSpeed(0);
				Motor.A.forward();
				Motor.C.forward();
		
			}
			else
			{
				Motor.A.setSpeed(0);
				Motor.C.setSpeed(900);
				Motor.A.forward();
				Motor.C.forward();
			}
		}
		
		

	}
	
	private void finishPreviousLevels() {
		LabyrinthSolver.setLevelCleared(true);
		SumpfSolver.setLevelCleared(true);
		RollBridgeSolver.setLevelCleared(true);
		DoorSolver.setLevelCleared(true);
		TreeAreaSolver.setLevelCleared(true);
		BridgeSolver.setLevelCleared(true);
		FallingBridgeSolver.setLevelCleared(true);
	}

}